#include < stdio.h>  
#include < opencv2\opencv.hpp>  
//#include < opencv2\features2d\features2d.hpp>  
//#include < opencv2\nonfree\nonfree.hpp>  
//#include < opencv2\nonfree\features2d.hpp>  

#include < vector>  

#ifdef _DEBUG  
#pragma comment(lib, "opencv_core246d.lib")   
#pragma comment(lib, "opencv_imgproc246d.lib")   //MAT processing  
//#pragma comment(lib, "opencv_objdetect246d.lib") //HOG  
//#pragma comment(lib, "opencv_gpu246d.lib")  
//#pragma comment(lib, "opencv_features2d246d.lib")  
#pragma comment(lib, "opencv_highgui246d.lib")  
//#pragma comment(lib, "opencv_ml246d.lib")  
//#pragma comment(lib, "opencv_stitching246d.lib")  
//#pragma comment(lib, "opencv_superres246d.lib")  
#pragma comment(lib, "opencv_calib3d246d.lib")  
//#pragma comment(lib, "opencv_nonfree246d.lib")  
//#pragma comment(lib, "opencv_flann246d.lib")  

#else  
#pragma comment(lib, "opencv_core246.lib")  
#pragma comment(lib, "opencv_imgproc246.lib")  
//#pragma comment(lib, "opencv_objdetect246.lib")  
//#pragma comment(lib, "opencv_gpu246.lib")  
//#pragma comment(lib, "opencv_features2d246.lib")  
#pragma comment(lib, "opencv_highgui246.lib")  
//#pragma comment(lib, "opencv_ml246.lib")  
//#pragma comment(lib, "opencv_stitching246.lib")  
//#pragma comment(lib, "opencv_superres246.lib")  
#pragma comment(lib, "opencv_calib3d246.lib")  
//#pragma comment(lib, "opencv_nonfree246.lib")  
//#pragma comment(lib, "opencv_flann246.lib")  

#endif  

using namespace std;  
using namespace cv;  

struct cornerInformation{  
	float x;  
	float y;  
	float x3;  
	float y3;  
	float z3;  
};  

void fprintMatrix(Mat matrix, string name);  
void fprintfVectorMat(vector< Mat> matrix, string name);  
void fprintf2Point( vector< vector< Point2f> > Points, string name);  
void fprintf3Point( vector< vector< Point3f> > Points, string name);  

#define Compute_intrinsic_Matrix
//#define Distort
//#define FilterFile

void main()  
{  
	//////////////////////////////////////////////////////////////////////////////////////////////////  
	int board_w, board_h;  
	int n_boards;  
	float measure=58;  
	Size imageSize;  

	vector< vector< Point2f> > imagePoints;  
	vector< vector< Point3f> > objectPoints;  

	/*printf("How many cross points of width direction? \n" );   
	scanf("%d", &board_w);  
	printf("How many cross points of Height direction? \n" );  
	scanf("%d", &board_h);  

	printf("How many board? (board will be read by this namimg-> ./pattern/p1.jpg, ./pattern/p2.jpg...\n");  
	scanf("%d", &n_boards);  

	printf("What mm ?\n");  
	scanf("%f", &measure);  

	printf("w=%d h=%d n=%d %lfmm\n", board_w, board_h, n_boards, measure);  */
	//////////////////////////////////////////////////////////////////////////////////////////////////  

	//////////////////////////////////////////////////////////////////////////////////////////////////  
	//image load  
	//extraction image point and object point
	int stopc = 0;
	char str[100]; 

	string fileName;
	string filePath = "D:\\!Projects\\!ExampleProjects\\Data\\HMI\\123_2_69\\";

	string fileIntrinsics = "Test123_2_69_50_Intrinsics.xml";
	string fileDistortion = "Test123_2_69_50_Distortion.xml";

	int iStart = 0;
	char *format = "%04d";
	string ext = ".png";
	//Set input params..  
	board_w=11;  
	board_h=7;  
	n_boards=50;  

	//int i = 0;
	int dem = 0;
	
	for(int i=0; dem < n_boards && stopc != 27; ++i)  
	{  
		//image load  
		sprintf(str, format, i + iStart); 
		
#ifdef FilterFile
		fileName = filePath + "Detected_" + str + ext;
		
		std::cout << std::endl << i << ".\t " << str << ext;

		Mat imgDetect = imread(fileName); 

		if (imgDetect.empty())
		{
			fileName = filePath + str + ext;
			Mat imgDel = imread(fileName);
			if (!imgDel.empty())
			{	
				imshow("Image is Deleted", imgDel);
				std::cout << " is Deleted!\t" << ++dem;
				std::remove(fileName.c_str());
				cv::waitKey(1);
			}	
		}
		if (i >= n_boards)
			break;
		//cv::waitKey(0);
#else
		fileName = filePath + str + ext;

		Mat img = imread(fileName);  
		if (img.empty())
			continue;
		
		std::cout << dem++ << ".\t " << str << ext << endl;

		imageSize = Size(img.cols, img.rows);  
		Mat gray;  
		cvtColor(img, gray, CV_RGB2GRAY);  
		vector< Point2f> corners;    

		//find chessboard corners  
		bool sCorner = findChessboardCorners(gray, Size(board_w, board_h), corners);  

		//if find corner success, then  
		if(sCorner)  
		{  
			//corner point refine  
			cornerSubPix(gray, corners, Size(11,11), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));  
			//draw corner  
			drawChessboardCorners(img, Size(board_w, board_h), corners, sCorner);  
			if(corners.size() == board_w*board_h)  
			{  
				vector< Point2f> v_tImgPT;  
				vector< Point3f> v_tObjPT;  
				//save 2d coordenate and world coordinate  
				for(int j=0; j< corners.size(); ++j)  
				{  
					Point2f tImgPT;  
					Point3f tObjPT;  

					tImgPT.x = corners[j].x;  
					tImgPT.y = corners[j].y;  

					tObjPT.x = j%board_w*measure;  
					tObjPT.y = j/board_w*measure;  
					tObjPT.z = 0;  

					v_tImgPT.push_back(tImgPT);  
					v_tObjPT.push_back(tObjPT);       
				}  
				imagePoints.push_back(v_tImgPT);  
				objectPoints.push_back(v_tObjPT); 
				
				fileName = filePath + "Detected_" + str + ext;
				imwrite(fileName, img);  

				imshow("Detected Image",img);  
				stopc = cvWaitKey(5);
			}  
		}  
#endif	
	}
#ifdef FilterFile
	std::cout << "\n" << dem << " file is Deleted Successed! Enter Keypressed to Stop!" << endl;
#endif
	//////////////////////////////////////////////////////////////////////////////////////////////////  

#ifdef Compute_intrinsic_Matrix
	//////////////////////////////////////////////////////////////////////////////////////////////////  
	//claibration part 
	printf(" *** Calibrating.....!\n\n");
	vector< Mat> rvecs, tvecs; 

	Mat intrinsic_Matrix(3,3, CV_64F);  
	Mat distortion_coeffs(8,1, CV_64F);  

	calibrateCamera(objectPoints, imagePoints, imageSize, intrinsic_Matrix, distortion_coeffs, rvecs, tvecs); 

	std::cout << intrinsic_Matrix << std::endl;
		
	for(int i=0; i< distortion_coeffs.rows; ++i)  
	{  
		for(int j=0; j< distortion_coeffs.cols; ++j)  
		{  
			printf("%lf ", distortion_coeffs.at< double>(i,j)); //cvmGet(matrix,i,j));  
		}  
		printf("\n");  
	}  
	printf("\n"); 
	printf(" *** Calibration Done!\n\n");
	////////////////////////////////////////////////////////////////////////////////////////////////////  
	////Save part  
	printf("Storing Intrinsics.xml and Distortions.xml files...\n");

	CvMat cvIntrinsics = intrinsic_Matrix;
	CvMat cvDistortion = distortion_coeffs;
	
	cvSave(fileIntrinsics.c_str(),&cvIntrinsics);
	cvSave(fileDistortion.c_str(),&cvDistortion);

	/*FileStorage fs("CamModel.yml", FileStorage::WRITE);

    fs << "frameCount" << n_boards;
    fs << "cameraMatrix" << intrinsic_Matrix << "distCoeffs" << distortion_coeffs;
    fs.release();*/

	printf("\n"); 
	printf(" *** Calibration Saved!\n\n");
	//////////////////////////////////////////////////////////////////////////////////////////////////// 
#else
	cv::waitKey(0);
#endif

#ifdef Distort
	//////////////////////////////////////////////////////////////////////////////////////////////////// 
	//------------------------- Video capture  output  undistorted ------------------------------
	Mat rview, map1, map2;

	initUndistortRectifyMap(intrinsic_Matrix, distortion_coeffs, Mat(),
		getOptimalNewCameraMatrix(intrinsic_Matrix, distortion_coeffs, imageSize, 1, imageSize, 0),
		imageSize, CV_16SC2, map1, map2);

	std::cout << "Enter Keypressed to Distort" << endl;
	cv::waitKey(0);

	dem = 0;
	for(int i=0; dem < n_boards && stopc != 27; ++i)  
	{  
		//image load
		sprintf(str, format, i + iStart); 
		fileName = filePath + str + ext;

		Mat view = imread(fileName); 
		if(view.empty())
			continue;

		
		std::cout << dem++ << ".\t " << str << ext << endl;

		remap(view, rview, map1, map2, INTER_LINEAR);

		Mat temp = view.clone();
		
		undistort(temp, view, intrinsic_Matrix, distortion_coeffs);
					
		//------------------------------ Show image and check for input commands -------------------
		fileName = filePath + "Detected_" + str + ext;
		Mat srcImage = imread(fileName); 

		imshow("Detected Image", srcImage);
		imshow("Distortiong Remap View", rview);
		imshow("Distortiong View", view);
		cv::waitKey(0);
	}
	////////////////////////////////////////////////////////////////////////////////////////////////////  
#endif
	
	cout << "Finished!!!" << endl;
	cvWaitKey(0);
}